Código
using LinearAlgebra
using Distributions
using Plots
using PlotThemes
theme(:wong2)Elementos de Matemáticas Aplicadas para Aplicaciones Tecnológicas
Se implementará un filtro de Kalman para fusionar los datos de sensores presentes en un vehículo para estimar la posición y orientación del mismo en dos dimensiones. Un diagrama del mismo se presenta en la Figura 1.
En la figura se representan los ejes \(x\) e \(y\), llamados el sistema de referencia inercial. Se repesenta además el eje \(x_b\), el cual junto con un eje \(y_b\) ortogonal al mismo forman el sistema de referencia del móvil, llamado body. El filtro de Kalman estimará la posición del móvil en el sistema de referencia intercial, \(x_{est},\, y_{est}\) así como la orientación del mismo, dada por el ángulo \(\theta\).
Los instrumentos disponibles para la estimación de la posición son los siguientes.
using LinearAlgebra
using Distributions
using Plots
using PlotThemes
theme(:wong2)Para cada dimensión de medición se considera su correspondiente error. Se hace a partir de los datos en la hoja de datos.
| Parámetro | Símbolo | Valor |
|---|---|---|
| Frecuencia de muestreo IMU | \(f_s\) | \(10 \, \mathrm{Hz}\) |
| Frecuencia de muestreo GPS | \(f_s'\) | \(1 \, \mathrm{Hz}\) |
| PSD ruido acelerómetro | \(S_{acc}\) | \(80\, \mathrm{\mu g}/\sqrt{\mathrm{Hz}}\) |
| PSD ruido giróscopo | \(S_{gir}\) | \(0.03\, ^\circ/s/\sqrt{\mathrm{Hz}}\) |
| PSD ruido giróscopo | \(S_{gir}\) | \(100\, \mathrm{\mu Gauss}/\sqrt{\mathrm{Hz}}\) |
| Error absoluto del GPS | \(\sigma_{GPS}\) | \(2.5\, \mathrm{m}\) |
| Intensidad campo magnético | \(B_T\) | \(25358\,\mathrm{nT}\) |
El error de las mediciones del IMU se calculan a partir de la densidad espectral del ruido informada en la hoja de datos, \(S_{acc}\) para el acelerómetro y \(S_{gir}\) para el giróscopo, y la frecuencia de muestreo \(f_s\)
\[ \sigma_{acc} = S_{acc} \sqrt{fs} \qquad \sigma_{acc} = S_{gir} \sqrt{fs} \]
El error del magnetómetro se calcula también a partir de su densidad espectral de ruido \(S_{mag}\) y su frecuencia de muestreo \(f_s'\), y también es afectado por la intensidad del campo magnético terrestre \(B_T\)
\[ \sigma_{mag} = \frac{S_{mag} \sqrt{fs'}}{B_T} \]
Fialmente, el error del GPS no depende de la frecuencia de muestreo, y es informado diréctamente en la hoja de datos
Δt = 0.1
fs = 10
fs2 = 1
PSD_a = 80*10*1e-6
PSD_g = 0.03*π/180
PSD_m = 100e-6*1e-4
B_T = 25358e-9
err_IMU_x = PSD_a*sqrt(fs)
err_IMU_θ = PSD_g*sqrt(fs)
err_GPS = 2.5
err_MAG = PSD_m*sqrt(fs2)/B_T
Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]
R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]
dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)El filtro de Kalman se hará sobre el estimador del estado de posición, velocidad, y orientación del móvil en el sistema de referencia inercial
\[ \mathbf{\hat{x}}_k = \begin{bmatrix}\hat{x}_k \\ \hat{\dot{x}}_k \\ \hat{y}_k \\ \hat{\dot{y}}_k \\ \hat{\theta}_k \end{bmatrix} \]
A cada instante de muestreo del IMU se obtiene un vector de mediciones \(\mathbf{\overline{u}}_k\), el cual se define como su valor real \(\mathbf{u}_k\) más su error \(\mathbf{w'}_k\)
\[ \mathbf{\overline{u}}_k = \mathbf{u}_k + \mathbf{w}_k' = \begin{bmatrix}u_k^{x_b} \\ u_k^{y_b} \\ u_k^{\theta} \end{bmatrix} + \mathbf{w}_k' \]
Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{w}'\) es dada por
\[ Q = \begin{bmatrix} (\sigma_{acc})^2 & 0 & 0\\ 0 & (\sigma_{acc})^2 & 0\\ 0 & 0 & (\sigma_{gir})^2\\ \end{bmatrix} \]
Cuando \(k\) corresponde a un instante de muestreo del GPS, se obtiene un vector de mediciones \(\mathbf{\overline{z}}_k\), el cual se define como su valor real \(\mathbf{z}_k\) más su error \(\mathbf{v}_k\)
\[ \mathbf{\overline{z}}_k = \mathbf{z}_k + \mathbf{v}_k = \begin{bmatrix}z_k^{x} \\ z_k^{y} \\ z_k^{\theta} \end{bmatrix} + \mathbf{v}_k \]
Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{w}'\) es dada por
\[ R = \begin{bmatrix} (\sigma_{GPS})^2 & 0 & 0\\ 0 & (\sigma_{GPS})^2 & 0\\ 0 & 0 & (\sigma_{mag})^2\\ \end{bmatrix} \]
En ausencia de aceleración, la evolución del estado en un tiempo \(\Delta t\) seguiría un movimiento rectilíneo uniforme, dado por
\[ \mathbf{x}_k = \begin{bmatrix}x_k \\ \dot{x}_k \\ y_k \\ \dot{y}_k \\ \theta_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & \Delta t & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}x_{k-1} \\ \dot{x}_{k-1} \\ y_{k-1} \\ \dot{y}_{k-1} \\ \theta_{k-1} \end{bmatrix} \]
La actualización del estado se hace en función de las mediciones de la IMU, que informan la aceleración del móvil en el sistema de referencia body, estas se modelan como el vector de medición \(\mathbf u\) más el vector de error \(\mathbf w'\)
Estas aceleraciones se transladan al sistema de referencia inercial y se utilizan para realizar la actualización por modelo del filtro de Kalman, dada por medio de la ecuación
\[ \mathbf{\hat{x}}_k = A\mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1} \]
En donde la matriz \(B_{k-1}\) que obtiene la actualización del estado en el sistema inercial \(\mathbf{x}\) en función de las mediciones de aceleración en el sistema body es dada por
\[ B_{k-1} = \begin{bmatrix} \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & - \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & 0 \\ \Delta t \cos(\theta_{k-1}) & - \Delta t \sin(\theta_{k-1}) & 0 \\ \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & 0 \\ \Delta t \sin(\theta_{k-1}) & \Delta t \cos(\theta_{k-1}) & 0 \\ 0 & 0 & \Delta t \end{bmatrix} \]
Y el ruido es similarmente transformado, obteniendo \(\mathbf{w}_{k-1} = B \mathbf{w}_{k-1}'\). La matriz de covarianza que resulta de la transformación es dada por
\[ Q_k = B Q B^T \]
La actualización por medición se basa en relacionar las mediciones del GPS y el magnetómetro con el estado actual \(\mathbf{\hat{x}}\) de la siguiente forma
\[ \mathbf{z}_k = H\mathbf{\hat{x}}_k - \mathbf{v}_k \]
En donde la matriz \(H\) es dada por
\[ H = \begin{bmatrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \]
Y el vector de ruido tiene matriz de covarianza \(R\)
A = [1 Δt 0 0 0;
0 1 0 0 0;
0 0 1 Δt 0;
0 0 0 1 0;
0 0 0 0 1]
function B_matrix(θ)
return [Δt^2*cos(θ)/2 -Δt^2*sin(θ)/2 0;
Δt*cos(θ) -Δt*sin(θ) 0;
Δt^2*sin(θ)/2 Δt^2*cos(θ)/2 0;
Δt*sin(θ) Δt*cos(θ) 0;
0 0 Δt]
end
H = [1 0 0 0 0;
0 0 1 0 0;
0 0 0 0 1]El algoritmo de estimación se implementa con una etapa de inicialización del estado \(\mathbf{\hat{x}}\) y de la matriz de covarianza \(P\), seguido de un ciclo for que realiza la predicción en cada instante de muestreo de la IMU, e incluye la etapa de corrección en cada instante de muestreo del GPS.
function kalman(t, tasa_GPS, u, z, Q, R)
x, P, K = kalman_initialize(t, z, R)
for k in 2:length(t)
x_est, P_est = kalman_predict(x[:,k-1], u[:,k], P[:,:,k-1], Q)
if k%tasa_GPS == 0
x_est, P_est, K_k = kalman_correct(x_est, z[:,k], P_est, R)
K[k] = norm(K_k)
end
x[:,k] = x_est
P[:,:,k] = P_est
end
return x, P, K
endkalman (generic function with 2 methods)
Los valores de posición y orientación del estado \(\mathbf{\hat{x}}\) se inicializan con las primeras mediciones provenientes del GPS/magnetómetro. Los valores de velocidad se inicializan en \(0\).
Se decide inicializar la matriz de covarianza con el error del GPS para la posición, el error del GPS multiplicado por la frecuencia de muestreo para la velocidad, y \(2\pi\) para la orientación.
function kalman_initialize(t, z, R)
N = length(t)
x = zeros(5, N)
x[1:2:5,1] = z[:,1]
P = zeros(5, 5, N)
P[:,:,1] = diagm([R[1,1]^2,(R[1,1]*fs2)^2,R[2,2]^2,(R[2,2]*fs2)^2,2pi])
K = zeros(N)
return x, P, K
endkalman_initialize (generic function with 1 method)
En cada instante de muestreo del IMU se realiza la etapa de predicción
function kalman_predict(x, u, P, Q)
B = B_matrix(x[5])
x_est = A*x + B*u
P_est = A*P*A' + B*Q*B'
return x_est, P_est
endkalman_predict (generic function with 1 method)
En cada instante de muestreo del GPS/magnetómetro, se aplica una corrección por medición al estimador.
function kalman_correct(x_prior, z, P_prior, R)
K = P_prior*H'*inv(H*P_prior*H'+R)
x_est = x_prior + K*(z-H*x_prior)
P_est = (I-K*H)*P_prior
return x_est, P_est, K
endkalman_correct (generic function with 3 methods)
En todas las trayectorias, los ruidos de los instrumentos se modelan como variables aleatorias gaussianas de media \(0\), con muestras independientes e idénticamente distribuidas.
\[ \mathbf{w}'_k \sim \mathcal{N}\left[\mathbf{0}, Q\right] \qquad \mathbf{v}_k \sim \mathcal{N}\left[\mathbf{0}, R\right] \]
T = 500
t = 0:0.1:T
u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)Se analiza la trayectoria real, dada por \(\mathbf{z}\), contra la trayectoria estimada extraida de \(\mathbf{\hat{x}}\) en el caso de que el vehículo se encuentra en reposo.
En este caso los vectores de medición \(\mathbf{\overline{u}}\) y \(\mathbf{\overline{z}}\) son dados por
\[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \qquad\qquad \mathbf{\overline{z}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{v}_k \]
function estatico_IMU()
return [0,0,0]
end
function estatico_GPS()
return [0,0,0]
end
u_true = hcat([estatico_IMU() for ti in t]...)
z_true = hcat([estatico_GPS() for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman(t, 10, u, z, Q, R)function trayectoria_xy(x, z_true)
p1 = plot(x[1,:], x[3,:], label="Estimación", xlabel="x", ylabel="y", aspect_ratio=:equal, title="Trayectoria XY")
plot!(p1, z_true[1,:], z_true[2,:], label="Trayectoria")
return p1
end
function trayectoria_xy(x, z_true, k_loss)
p1 = plot(x[1,:], x[3,:], label="Estimación", xlabel="x", ylabel="y", aspect_ratio=:equal, title="Trayectoria XY")
plot!(p1, x[1,k_loss], x[3,k_loss], aspect_ratio=:equal, label="Pérdida instrumento")
plot!(p1, z_true[1,:], z_true[2,:], label="Trayectoria")
return p1
end
trayectoria_xy(x, z_true)function trayectoria_t(t, x, z_true)
p1 = plot(t, x[1,:], ylabel="x [m]", title="Trayectoria en función del tiempo")
plot!(p1, t, z_true[1,:])
p2 = plot(t, x[3,:], ylabel="y [m]")
plot!(p2, t, z_true[2,:])
p3 = plot(t, x[5,:] .% 2pi, ylabel="θ", xlabel="t [s]")
plot!(p3, t, z_true[3,:] .% 2pi)
return plot(p1,p2,p3, layout=(3,1), legend=false)
end
function trayectoria_t(t, x, z_true, k_loss)
p1 = plot(t, x[1,:], ylabel="x [m]", title="Trayectoria en función del tiempo")
plot!(p1, t[k_loss], x[1,k_loss])
plot!(p1, t, z_true[1,:])
p2 = plot(t, x[3,:], ylabel="y [m]")
plot!(p2, t[k_loss], x[3,k_loss])
plot!(p2, t, z_true[2,:])
p3 = plot(t, x[5,:] .% 2pi, ylabel="θ", xlabel="t [s]")
plot!(p3, t[k_loss], x[5,k_loss] .% 2pi)
plot!(p3, t, z_true[3,:] .% 2pi)
return plot(p1,p2,p3, layout=(3,1), legend=false)
end
trayectoria_t(t, x, z_true)Se analiza la trayectoria real, dada por \(\mathbf{z}\), contra la trayectoria estimada extraida de \(\mathbf{\hat{x}}\) en el caso de que el vehículo parte del reposo y sigue un movimiento rectilíneo uniformemente acelerado con aceleración \(a\) a un ángulo \(\theta\) respecto a la horizontal.
En este caso los vectores de medición \(\mathbf{\overline{u}}\) y \(\mathbf{\overline{z}}\) son dados por \[ \mathbf{\overline{u}}_k = \begin{bmatrix} a \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \qquad\qquad \mathbf{\overline{z}}_k = \begin{bmatrix} \frac 1 2 a t_k^2 \cos(\theta) \\ \frac 1 2 a t_k^2 \sin(\theta) \\ \theta \end{bmatrix} + \mathbf{v}_k \]
function mrua_IMU(a)
return [a,0,0]
end
function mrua_GPS(a, t, θ)
return [1/2 * a * t^2 * cos(θ), 1/2 * a* t^2 * sin(θ), θ]
end
u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman(t, 10, u, z, Q, R)trayectoria_xy(x, z_true)trayectoria_t(t, x, z_true)Se analiza la trayectoria real, dada por \(\mathbf{z}\), contra la trayectoria estimada extraida de \(\mathbf{\hat{x}}\) en el caso de que el vehículo sigue un movimiento circular uniforme a un radio \(r\) del origen y con velocidad angular \(\omega\).
En este caso los vectores de medición \(\mathbf{\overline{u}}\) y \(\mathbf{\overline{z}}\) son dados por \[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ \omega^2 r \\ \omega \end{bmatrix} + \mathbf{w}_k' \qquad\qquad \mathbf{\overline{z}}_k = \begin{bmatrix} r \cos(\omega t_k) \\ r \sin(\omega t_k) \\ \omega t_k + \frac \pi 2 \end{bmatrix} + \mathbf{v}_k \]
function circular_IMU(r, w, t)
return [0,w^2*r,w]
end
function circular_GPS(r, w, t)
return [r*cos(w*t),r*sin(w*t),w*t+π/2]
end
u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman(t, 10, u, z, Q, R)trayectoria_xy(x, z_true)trayectoria_t(t, x, z_true)En la comparación de las trayectorias reales contra la estimación se observa que el error de medición es máximo al inicio de la trayectoria, y este error disminuye conforme el tiempo avanza y el filtro de Kalman alcanza un estado de equilibrio. Se busca analizar cuantitativamente este efecto comparando el error de la medición y la incerteza de la estimación en función del tiempo.
Se procede a cuantificar el error de la estimación, el cual se define como la norma cuadrática de la distancia entre la estimación y el valor real
\[ \epsilon_k = \left\lVert\begin{bmatrix}\hat{x}_k\\\hat{y}_k\end{bmatrix}- \begin{bmatrix}z^x_k\\ z^y_k\end{bmatrix}\right\rVert^2 \]
function error_cuadratico(x, z_true)
xs = [[x_i[1] x_i[3]] for x_i in eachcol(x)]
zs = [[z_i[1] z_i[2]] for z_i in eachcol(z_true)]
return [norm(x_i-z_i)^2 for (x_i, z_i) in zip(xs, zs)]
enderror_cuadratico (generic function with 1 method)
Esto se compara con la incerteza del estimador, la cual se cuantifica con la traza de la matriz de covarianza \(P\)
function varianza_estimador(P)
return [tr(P[:,:,i]) for i in 1:size(P,3)]
endvarianza_estimador (generic function with 1 method)
Los resultados verifican las observaciones, y se presentan a continuación
function plot_error(t, x, z_true, P)
v = varianza_estimador(P)
e = error_cuadratico(x, z_true)
p1 = plot(t, v, ylim=[0, min(100,maximum(v)*1.1)], ylabel="Incerteza")
p2 = plot(t, e, ylim=[0, 30], ylabel="Error cuadrático", xlabel="t")
return plot(p1, p2, layout=(2,1))
end
function plot_error(t, x, z_true, P, k_loss)
v = varianza_estimador(P)
e = error_cuadratico(x, z_true)
p1 = plot(t, v, ylim=[0, min(100,maximum(v)*1.1)], ylabel="Incerteza")
plot!(p1, t[k_loss], v[k_loss], label="Pérdida Instrumento")
p2 = plot(t, e, ylim=[0, 30], ylabel="Error cuadrático", xlabel="t")
plot!(p2, t[k_loss], e[k_loss], label="Pérdida Instrumento")
return plot(p1, p2, layout=(2,1))
end
plot_error(t, x, z_true, P)Además del error de medición, se analiza la evolución de la ganancia de Kalman \(K_k\) en función del tiempo. Esta se cuantifica como la norma de la matriz.
function plot_gain(T, P, K)
k = findall(x -> x!=0, K)
v = varianza_estimador(P)
p1 = plot(t, v, ylabel="Incerteza", linetype=:steppost)
p2 = plot(t[k], K[k], ylim=[0, maximum(K)*1.1], ylabel="Ganancia Kalman", xlabel="t", linetype=:steppost)
return plot(p1, p2, layout=(2,1))
end
function plot_gain(t, P1, K1, P2, K2)
v1 = varianza_estimador(P1)
v2 = varianza_estimador(P2)
k1 = findall(x -> x!=0, K1)
k2 = findall(x -> x!=0, K2)
p1 = plot(t, v1, ylabel="Incerteza", linetype=:steppost, label=false)
plot!(t, v2, ylabel="Incerteza", linetype=:steppost, label="Referencia", ylim=(0,100))
p2 = plot(t[k1], K1[k1], ylim=[0, max(maximum(K1),maximum(K2))*1.1], ylabel="Ganancia Kalman", xlabel="t", linetype=:steppost, label=false)
plot!(t[k2], K2[k2], ylim=[0, max(maximum(K1),maximum(K2))*1.1], ylabel="Ganancia Kalman", xlabel="t", linetype=:steppost, label="Referencia")
return plot(p1, p2, layout=(2,1))
end
plot_gain(t, P, K)Se hace un foco en el estado estacionario del filtro, para observar como es afectada la incerteza del estimador y la ganacia de Kalman en los instantes en los que se registra una medición del GPS.
k1 = 4000
k2 = 4050
plot_gain(t[k1:k2], P[:,:,k1:k2], K[k1:k2])A continuación se analizará como se modifica el desempeño del filtro si se varían los parámetros de error de los instrumentos que lo componen, ya sea el IMU o el GPS. Se llama caso de referencia al filtro de Kalman desarrollado.
Primero se analiza que sucede si el error del IMU se mantiene igual y el error del GPS incrementa 5 veces. Esto se realiza ejecutando el programa pero utilizando la siguiente matriz de covariaza para el ruido de medición
\[ R = \begin{bmatrix} (5\sigma_{GPS})^2 & 0 & 0\\ 0 & (5\sigma_{GPS})^2 & 0\\ 0 & 0 & (\sigma_{mag})^2\\ \end{bmatrix} \]
Se analiza las trayectorias rectilínea y circular, y los resultados se presentan a continuación.
Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]
R = [(5*err_GPS)^2 0 0; 0 (5*err_GPS)^2 0; 0 0 err_MAG^2]
dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)
u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)
u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
x, P2, K2 = kalman(t, 10, u, z, Q, R)trayectoria_xy(x, z_true)trayectoria_t(t, x, z_true)u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
x, P2, K2 = kalman(t, 10, u, z, Q, R)trayectoria_xy(x, z_true)trayectoria_t(t, x, z_true)plot_error(t, x, z_true, P2)k1 = 4800
k2 = 4850
plot_gain(t[k1:k2], P2[:,:,k1:k2], K2[k1:k2], P[:,:,k1:k2], K[k1:k2])A continuación se analiza que sucede si el error del IMU es el que incrementa 5 veces y el error del GPS se mantiene igual. Esto se realiza ejecutando el programa pero utilizando la siguiente matriz de covariaza para el ruido de modelo
\[ Q = \begin{bmatrix} (5\sigma_{acc})^2 & 0 & 0\\ 0 & (5\sigma_{acc})^2 & 0\\ 0 & 0 & (5\sigma_{gir})^2\\ \end{bmatrix} \]
Q = [(5*err_IMU_x)^2 0 0; 0 (5*err_IMU_x)^2 0; 0 0 (5*err_IMU_θ)^2]
R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]
dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)
u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)
u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
x, P2, K2 = kalman(t, 10, u, z, Q, R)trayectoria_xy(x, z_true)trayectoria_t(t, x, z_true)u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
x, P2, K2 = kalman(t, 10, u, z, Q, R)trayectoria_xy(x, z_true)trayectoria_t(t, x, z_true)plot_error(t, x, z_true, P2)k1 = 4000
k2 = 4050
plot_gain(t[k1:k2], P2[:,:,k1:k2], K2[k1:k2], P[:,:,k1:k2], K[k1:k2])Suponemos la pérdida del GPS. En este caso la ecuación de acualización de estado se convierte en
\[ \mathbf{z}_k' = H'\mathbf{\hat{x}}_k - \mathbf{v}_k' \]
En donde la matriz \(H\) es dada por
\[ H' = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 \end{bmatrix} \]
\[ \mathbf{z}_k' = \begin{bmatrix} z_k^{\theta} \end{bmatrix} \]
Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{v}'\) es dada por
\[ R' = \begin{bmatrix} (\sigma_{mag})^2\\ \end{bmatrix} \]
Suponemos la pérdida del GPS. En este caso la ecuación de acualización de estado se convierte en
\[ \mathbf{z}_k'' = H''\mathbf{\hat{x}}_k - \mathbf{v}_k'' \]
En donde la matriz \(H\) es dada por
\[ H'' = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \end{bmatrix} \]
\[ \mathbf{z}_k'' = \begin{bmatrix} z_k^{x} \\ z_k^{y} \end{bmatrix} \]
Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{v}'\) es dada por
\[ R'' = \begin{bmatrix} (\sigma_{GPS})^2 & 0\\ 0 & (\sigma_{GPS})^2 \end{bmatrix} \]
function kalman_correct(x_prior, z, P_prior, K, GPS=true, mag=true)
if GPS && mag
H2 = H
z2 = z
R2 = R
elseif mag
H2 = H[3,:]'
z2 = z[3]
R2 = R[3,3]
else
H2 = H[1:2, :]
z2 = z[1:2]
R2 = R[1:2,1:2]
end
K = P_prior*H2'*inv(H2*P_prior*H2'+R2)
x_est = x_prior + K*(z2-H2*x_prior)
P_est = (I-K*H2)*P_prior
return x_est, P_est, K
end
function kalman(t, tasa_GPS, u, z, Q, R, gps, mag)
x, P, K = kalman_initialize(t, z, R)
K = zeros(size(t))
for k in 2:length(t)
x_est, P_est = kalman_predict(x[:,k-1], u[:,k], P[:,:,k-1], Q)
if k%tasa_GPS == 0
x_est, P_est, K_k = kalman_correct(x_est, z[:,k], P_est, R, gps[k], mag[k])
K[k] = norm(K_k)
end
x[:,k] = x_est
P[:,:,k] = P_est
end
return x, P, K
endkalman (generic function with 2 methods)
Q = [(err_IMU_x)^2 0 0; 0 (err_IMU_x)^2 0; 0 0 (err_IMU_θ)^2]
R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]
dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)
u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)
u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)
u = u_true + u_noise
z = z_true + z_noise
k_loss = 2001:4000
gps_sig = ones(size(t)) .|> Bool
mag_sig = ones(size(t)) .|> Bool
gps_sig[k_loss] = zeros(length(k_loss)) .|> Bool
x, P2, K2 = kalman(t, 10, u, z, Q, R, gps_sig, mag_sig)trayectoria_xy(x, z_true, k_loss)trayectoria_t(t, x, z_true, k_loss)plot_error(t, x, z_true, P2, k_loss)gps_sig = ones(size(t)) .|> Bool
mag_sig = ones(size(t)) .|> Bool
mag_sig[k_loss] = zeros(length(k_loss)) .|> Bool
x, P2, K2 = kalman(t, 10, u, z, Q, R, gps_sig, mag_sig)([20.795148915177673 20.795117125888112 … 20.242389488641933 20.242765609415997; 0.0 -0.0006357857912123561 … 0.003717783652726088 0.003804631828543173; … ; 0.0 -0.0004717493715834802 … 0.2581043301022885 0.25789636057409177; 1.570159913381644 1.5716896101676954 … 7.852512900119905 7.853581883708354], [39.0625 0.0 … 0.0 0.0; 0.0 39.0625 … 0.0 0.0; … ; 0.0 0.0 … 39.0625 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 39.45312500016 3.9062500032 … 1.2916044610942532e-29 0.0; 3.9062500032 39.062500064 … 5.243052355554181e-27 0.0; … ; 1.9940497855492672e-28 -3.284111285018992e-27 … 39.062500064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 40.6250000016 7.8125000128 … 5.926439159334028e-28 0.0; 7.8125000128 39.062500127999996 … -3.1064992711983e-27 0.0; … ; 1.7851743431851958e-28 1.3947598470494988e-27 … 39.062500127999996 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 0.15972723750735476 0.00202037050907998 … -6.442695314285448e-22 0.0; 0.0020203705090799796 5.085414131495572e-5 … -2.78373821095346e-23 0.0; … ; 1.6610559400219101e-21 6.533362657764144e-23 … 5.085414131495572e-5 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 0.1561315593807332 0.0019748610285924453 … -6.248445440813035e-22 0.0; 0.001974861028592445 5.027814126896574e-5 … -2.7409106120205694e-23 0.0; … ; 1.609295372519169e-21 6.426784175872226e-23 … 5.027814126896574e-5 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 0.1565270345278644 0.0019798920427193415 … -6.275858592719784e-22 0.0; 0.001979892042719341 5.034214126896574e-5 … -2.7409370805685353e-23 0.0; … ; 1.6157218015671055e-21 6.427381424130635e-23 … 5.034214126896574e-5 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7623134290242808 … 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7131175945847074, 0.0])
trayectoria_xy(x, z_true, k_loss)trayectoria_t(t, x, z_true, k_loss)plot_error(t, x, z_true, P2, k_loss)